#ifndef __A_STAR_NODE_H__
#define __A_STAR_NODE_H__

#include <ros/ros.h>
#include <ros/package.h>
#include <tf2/utils.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/GridCells.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>

#include <iostream>
#include <sstream>
#include <opencv2/opencv.hpp>
#include "../a_star/a_star.h"
#include "../jps/jump_point_search.h"
#include "../common/map_helper.h"
using namespace std;
using namespace cv;


enum class PathPlanningType{
    A_STAR = 0,
    JPS
};

class GlobalPlanning{
    public:
        GlobalPlanning();
        ~GlobalPlanning();

        // 初始化算法
        void InitPlanning();
        // 设置地图数据
        void SetMap();
        // geometry_msgs::Pose -> cv::Point
        cv::Point Pose2CvPoint(geometry_msgs::Pose pose);
        // cv::Point -> geometry_msgs::Pose
        geometry_msgs::Pose CvPoint2Pose(cv::Point point);
        // 发布原始路径信息
        void PublishPathOrigin(vector<cv::Point>& path_points);
        // 发布扩展点地图信息
        void PublishExpendPointsMap(vector<cv::Point>& expand_poinds);

        /*********************************************************************
         * Astar
         **********************************************************************/
        // Astar 初始化
        void AstarInit();
        // 设置地图数据
        void SetAstarMap(cv::Mat &image);
        // Astar 路径规划
        void AstarPath(geometry_msgs::Pose start, geometry_msgs::Pose end);

        /*********************************************************************
         * JPS
         **********************************************************************/
        // Astar 初始化
        void JPSInit();
        // 设置地图数据
        void SetJPSMap(cv::Mat &image);
        // Astar 路径规划
        void JPSPath(geometry_msgs::Pose start, geometry_msgs::Pose end);
        // 发布跳点数据
        void PublishJumpPoints(vector<cv::Point>& jump_points);

    private:
        // 地图回调函数
        void MapCallback(const nav_msgs::OccupancyGridConstPtr& msg);
        // Rviz位姿回调函数
        void RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg);

    private:
        ros::NodeHandle __nh;
        ros::Publisher __path_pub;
        ros::Publisher __start_pose_pub;
        ros::Publisher __goal_pose_pub;
        ros::Publisher __expand_points_pub;
        ros::Publisher __jps_jump_points_pub;
        ros::Publisher __map_erode_pub;
        ros::Subscriber __map_sub;
        ros::Subscriber __rviz_pose_sub;

        float __robot_radius;       //机器人半径
        boost::shared_ptr<const nav_msgs::OccupancyGrid> __map_ptr;
        cv::Mat __map_image;
        vector<geometry_msgs::PoseStamped> __rviz_poses;
        string __output_dir;

    private:
        PathPlanningType __path_planning_type;  //路径生成方式
        MapHelper __map_helper;  
        //Planner
        AstarPlanner __astar_planner;
        JumpPointSearchPlanner __jps_planner;
};

#endif